/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/
//#include "client.h"

#include <arpa/inet.h>
#include <ctype.h>
#include <errno.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>

#include "commandStruct.h"
#include "NIDAQmxBase.h"


#define DAQmxErrChk(functionCall) { if( DAQmxFailed(error=(functionCall)) ) { goto Error; } }

void writeNIDAQ(uInt16 *buf, int len) {
   // Task parameters
   int32       error = 0;
   TaskHandle  taskHandle = 0;
   char        errBuff[2048];

   // Channel parameters
   char        chan[] = "Dev1/port0:1";

   // Write parameters
   uInt16      w_data [2];
   int32       written;


   // Create Digital Output (DO) Task and Channel
   DAQmxErrChk (DAQmxBaseCreateTask ("", &taskHandle));
   DAQmxErrChk (DAQmxBaseCreateDOChan(taskHandle,chan,"",DAQmx_Val_ChanForAllLines));

   // Start Task (configure port)
   //DAQmxErrChk (DAQmxBaseStartTask (taskHandle));

   //  Write 0x55 to port(s)
   //  Only 1 sample per channel supported for static DIO
   //  Autostart ON

   w_data[0] = 0x0000;
   w_data[1] = buf[0];

   printf("Data to write: 0x%X\n", w_data[1]);

   DAQmxErrChk (DAQmxBaseWriteDigitalU8(taskHandle,2,1,10.0,DAQmx_Val_GroupByChannel,(uInt8*)&w_data[0],&written,NULL));

   printf("success!\n");

Error:

   if (DAQmxFailed (error))
      DAQmxBaseGetExtendedErrorInfo (errBuff, 2048);

   if (taskHandle != 0)
   {
      DAQmxBaseStopTask (taskHandle);
      DAQmxBaseClearTask (taskHandle);
   }

   if (error)
      printf ("DAQmxBase Error %d: %s\n", (int)error, errBuff);
}


int main(int argc, char **argv) {
  int i, r, port, sock;
  char host[100];
  uInt16 init_buf[1];
  struct sockaddr_in server, client;
  struct hostent *me, *che;

  init_buf[0] = 3640;

  if (argc == 1) {
    printf("need hostname port args: hostname:port, exiting\n");
    exit(EXIT_FAILURE);
  }

  /* this stuff is all for parsing the command line components: 
   * the server hostname/ip address and the port number        */
  if (argc == 2) {
    char* split;
    int colon;
    
    split = index(argv[1], ':');

    colon = strlen(argv[1]) - strlen(split);
    strncpy(host, argv[1], colon);
    host[colon] = '\x00';
    port = atoi(++split);

  } else {
    for (i=1; i<argc; i++) {
      if ((strcmp(argv[i], "-h") == 0) || 
        (strcmp(argv[i], "--hostname") == 0)) {
        strcpy(host, argv[i+1]);
      } else if ((strcmp(argv[i], "-p") == 0) || 
        (strcmp(argv[i], "--port-number") == 0)) {
        if (argv[i+1] != NULL)
          strcpy(host, argv[i+1]);
        port = atoi(argv[i+1]);
      }
    }
  }

  /* set NIDAQ to off state */
  writeNIDAQ(&init_buf[0], 1);

  /* clearing everything.... */
  memset(&server, 0, sizeof(server));
  
  /* Setup the TCP socket */
  sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
  if (sock < 0) {
    printf("unable to create socket: %s\n", strerror(errno));
    exit(EXIT_FAILURE);
  } 

  /* set up the server's ip addr & port */
  server.sin_family = AF_INET;
  if ((server.sin_port = htons((unsigned short)port)) == 0) {
    printf("Can't get port num \'%d\'\n", port);
    exit(EXIT_FAILURE);
  }
  server.sin_addr.s_addr = inet_addr(host);

  /* connect to the server */ 
  if ( connect(sock, (struct sockaddr*)&server, sizeof(server)) != 0) {
    printf("unable to connect to server: %s\n", strerror(errno));
    exit(EXIT_FAILURE);
  }

  /* when we're finished connecting we go to talk mode */
  while (1) {
    struct timeval timeout;
    fd_set read_fds;

    timeout.tv_sec = 5;
    timeout.tv_usec = 0;

    FD_ZERO(&read_fds);
    FD_SET(sock, &read_fds);
    
    r = select(sock+1, &read_fds, NULL, NULL, &timeout);

    if (r < 0) {
      printf("select error: %s", strerror(errno));
      continue;
    }
    
    if (FD_ISSET(sock, &read_fds)) {
      uint16_t buf[2];
      r = recv(sock, &buf[0], 2, 0);

      if (r > 0) {
        printf("received %X\n", *(uint16_t*)&buf[0]);
        writeNIDAQ((uInt16*)buf, 2);
      }
    }
  }

  exit(EXIT_SUCCESS);
}
